package math;

public class EulerAngles {
	private float heading, pitch, bank; // y x z all in Math3D.DEGREEs

	/* facing -z direction */
	public EulerAngles() {
		identity();
	}

	public EulerAngles(float h, float p, float b) {
		this.heading = h;
		this.pitch = p;
		this.bank = b;
	}

	public void Set(EulerAngles ea) {
		this.heading = ea.heading;
		this.pitch = ea.pitch;
		this.bank = ea.bank;
	}

	public void rotate(EulerAngles rot) {
		heading += rot.heading;
		pitch += rot.pitch;
		bank += rot.bank;

		if (pitch >= Math3D.D_360 || pitch <= -Math3D.D_360) {
			pitch = pitch % Math3D.D_360;
		}

		if (heading >= Math3D.D_360 || heading <= -Math3D.D_360) {
			heading = heading % Math3D.D_360;
		}

		if (bank >= Math3D.D_360 || bank <= -Math3D.D_360) {
			bank = bank % Math3D.D_360;
		}

		if (pitch <= -Math3D.D_090) {
			pitch = -Math3D.D_090;
		} else if (pitch >= Math3D.D_090) {
			pitch = Math3D.D_090;
		}
	}

	public Vector3f toDirection() {
		double h = heading * Math3D.R_PI_OVER_D_180;
		double p = pitch * Math3D.R_PI_OVER_D_180;

		float sinH = (float) Math.sin(h);
		float cosH = (float) Math.cos(h);
		float cosP = (float) Math.cos(p);
		float sinP = (float) Math.sin(p);
		return new Vector3f(cosP * sinH, sinP, -cosP * cosH);
	}

	public Matrix4 toLookAtMatrix() {
		Vector3f direction = toDirection();
		Vector3f rightDirection = direction.cross(Vector3f.NORMAL_Y).normalize();
		Vector3f perpUpDirection = rightDirection.cross(direction);

		Matrix4 rotMat = new Matrix4().clearToIdentity();
		rotMat.putColumn(0, rightDirection, 0);
		rotMat.putColumn(1, perpUpDirection, 0);
		rotMat.putColumn(2, direction.mult(-1), 0);
		return rotMat.transpose();
	}

	private void identity() {
		heading = 0;
		pitch = 0;
		bank = 0;
	}

	public void heading(float heading) {
		this.heading += heading;
		MathUtil.clamp(this.heading, -88.75f, -1);
	}

	public void pitch(float pitch) {
		this.pitch += pitch;
	}

	public void bank(float bank) {
		this.bank += bank;
	}

	public float getHeadingRadians() {
		return heading * Math3D.R_PI_OVER_D_180;
	}

	public float getPitchRadians() {
		return pitch * Math3D.R_PI_OVER_D_180;
	}

	public float getBankRadians() {
		return bank * Math3D.R_PI_OVER_D_180;
	}

	public float getHeading() {
		return heading;
	}

	public float getPitch() {
		return pitch;
	}

	public float getBank() {
		return bank;
	}

	public String println() {
		return " heading" + this.heading + " pitch" + this.pitch + " bank" + this.bank;
	}
}
